/*
 * SUDSSensorMinion.cpp
 *
 *  Created on: Jun 22, 2011
 *      Author: jrodrig9
 */

#include "SUDSSensorMinion.h"
#include <time.h>
#include <stdio.h>
#include <unistd.h>
#include <stdlib.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/ioctl.h>
#include <fcntl.h>
#include <linux/fs.h>
#include <errno.h>
#include <string.h>

SUDSSensorMinion::SUDSSensorMinion() {
	leftSensorState = -1;
	rightSensorState = -1;

	detection_threshold = 683; // 2/3 of 1024 (Workspace of ADC)
	potential_threshold = 341; // 1/3 of 1024 (Workspace of ADC)

	time(&start);
}

int SUDSSensorMinion::getLeftSensorState() {
	return leftSensorState;
}

int SUDSSensorMinion::getRightSensorState() {
	return rightSensorState;
}

void SUDSSensorMinion::checkSensors() {

	//int temp[2] = getRealData();
	int temp[2] = &getFakeData();

	leftSensorState = temp[0];
	rightSensorState = temp[1];
}

int* SUDSSensorMinion::getFakeData() {

	int leftsensor = 0;
	int rightsensor = 0;
	time_t rightnow;
	double diff;
	int outputhelper[2] = {0,0};
	int* output;
	output = outputhelper;

	time(&rightnow);
	diff = difftime(rightnow,start);
	if(diff > 2){
		leftsensor = rand() % 3;
		rightsensor = rand() % 3;
		time(&start);
	}
	else{
		leftsensor = leftSensorState;
		rightsensor = rightSensorState;
	}

	outputhelper[0] = leftsensor;
	outputhelper[1] = rightsensor;

	return output;
}

int* SUDSSensorMinion::getRealData() {

	int output[2] = {3,3};

	initalize_GPIOs();

	/*
	char gpb0_out[5];
	char gpb1_out[5];

	//Edit the string with the correct directory
	if ((fp1 = fopen("/sys/class/gpio/GPB0/value", "rb+")) == NULL)
	{
		exit(1);
	}
	//Set pointer to beginning of the file
	rewind(fp1);
	//Write our value of "out" to the file
	fread(&gpb0_out, sizeof(char), 5, fp1);
	fclose(fp1);

	//Edit the string with the correct directory
	if ((fp2 = fopen("/sys/class/gpio/GPB1/value", "rb+")) == NULL)
	{
		exit(1);
	}
	//Set pointer to beginning of the file
	rewind(fp2);
	//Write our value of "out" to the file
	fread(&gpb1_out, sizeof(char), 5, fp2);
	fclose(fp2);

	//Reclassify whats read on the GPIO pins as ints
	if(gpb0_out[0] == '1')
		output[0] = 1;
	else if (gpb0_out[0] == '0')
		output[0] = 0;
	if (gpb1_out[0] == '1')
		output[1] = 1;
	else if (gpb1_out[0] == '0')
		output[1] = 0;
	*/

	return output;
}

void SUDSSensorMinion::initalize_GPIOs(){
/*
	//Need to edit the Pin Numbers to write to GPB0 & GPB 1
	char gpb0 = '32';
	char gpb1 = '33';

	//Temp variable to store what to write to file
	char set_value[4];

	//ACCESS PINS
	//Doing this Process twice. Once for each of the GPIO pins to use
	if ((fp1 = fopen("/sys/class/gpio/export", "ab")) == NULL)
		{
			exit(1);
		}
	//Set pointer to beginning of the file
	rewind(fp1);
	//Write our value for gpb0 to the file
	set_value[0] = gpb0;
	fwrite(&set_value, sizeof(char), 1, fp1);
	set_value[1] = gpb1;
	fwrite(&set_value, sizeof(char), 1, fp1);
	//Close the file
	fclose(fp1);

	//SET DIRECTION

	//Edit the string with the correct directory
	if ((fp1 = fopen("/sys/class/gpio/GPB0/direction", "rb+")) == NULL)
	{
		exit(1);
	}
	//Set pointer to beginning of the file
	rewind(fp1);
	//Write our value of "out" to the file
	strcpy(set_value,"in");
	fwrite(&set_value, sizeof(char), 2, fp1);
	fclose(fp1);

	//Edit the string with the correct directory
	if ((fp2 = fopen("/sys/class/gpio/GPB1/direction", "rb+")) == NULL)
	{
		exit(1);
	}
	//Set pointer to beginning of the file
	rewind(fp2);
	//Write our value of "out" to the file
	strcpy(set_value,"in");
	fwrite(&set_value, sizeof(char), 2, fp2);
	fclose(fp2);
*/
}

int SUDSSensorMinion::check_against_threshold(int in){

	if(in > potential_threshold){
		if(in > detection_threshold)
			return 2;
		else
			return 1;
	}
	else
		return 0;

}
